#include "HandEyeCalibration.h"


// cv::Mat R_T2RT(cv::Mat& R, cv::Mat& t)
// {
//     cv::Mat RT;
//     cv::Mat RT_left(4, 3, CV_64F), RT_right(4, 1, CV_64F);

//     RT_left << R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
//             R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
//             R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2),
//             0.0, 0.0, 0.0;
//     RT_right << t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0), 1.0;
//     cv::hconcat(RT_left, RT_right, RT);
//     return RT;
// }

cv::Mat eulerAngle2RotMat(cv::Mat& eulerAngle, std::string& seq)
{
    eulerAngle /= 180 / CV_PI;
    cv::Matx13d m(eulerAngle);
    auto xAngle = m(0, 0), yAngle = m(0, 1), zAngle = m(0, 2);
	auto xs = std::sin(xAngle), xc = std::cos(xAngle);
	auto ys = std::sin(yAngle), yc = std::cos(yAngle);
	auto zs = std::sin(zAngle), zc = std::cos(zAngle);
 
	cv::Mat rotX = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, xc, -xs, 0, xs, xc);
	cv::Mat rotY = (cv::Mat_<double>(3, 3) << yc, 0, ys, 0, 1, 0, -ys, 0, yc);
	cv::Mat rotZ = (cv::Mat_<double>(3, 3) << zc, -zs, 0, zs, zc, 0, 0, 0, 1);

    cv::Mat rotMat;
    rotMat = rotX * rotY * rotZ;
    return rotMat;

}

cv::Mat eulerZXZ2RotMat(double alpha, double beta, double gamma)
{
    cv::Mat R_x1 = cv::Mat::eye(3, 3, CV_64F);
	cv::Mat R_y = cv::Mat::eye(3, 3, CV_64F);
	cv::Mat R_x2 = cv::Mat::eye(3, 3, CV_64F);
 
	// 第一次绕Z轴旋转
	R_x1.at<double>(0, 0) = cos(alpha);
	R_x1.at<double>(0, 1) = -sin(alpha);
	R_x1.at<double>(1, 0) = sin(alpha);
	R_x1.at<double>(1, 1) = cos(alpha);
 
	// 绕X轴旋转
	R_y.at<double>(1, 1) = cos(beta);
	R_y.at<double>(1, 2) = -sin(beta);
	R_y.at<double>(2, 1) = sin(beta);
	R_y.at<double>(2, 2) = cos(beta);
 
	// 第二次绕Z轴旋转
	R_x2.at<double>(0, 0) = cos(gamma);
	R_x2.at<double>(0, 1) = -sin(gamma);
	R_x2.at<double>(1, 0) = sin(gamma);
	R_x2.at<double>(1, 1) = cos(gamma);
 
	return R_x1 * R_y * R_x2;
}

cv::Mat eulerXYZ2RotMat(double alpha, double beta, double gamma)
{
    cv::Mat R_x = cv::Mat::eye(3, 3, CV_64F);
	cv::Mat R_y = cv::Mat::eye(3, 3, CV_64F);
	cv::Mat R_z = cv::Mat::eye(3, 3, CV_64F);
 
	// 第一次绕x轴旋转
	R_x.at<double>(1, 1) = cos(alpha);
	R_x.at<double>(1, 2) = -sin(alpha);
	R_x.at<double>(2, 1) = sin(alpha);
	R_x.at<double>(2, 2) = cos(alpha);
 
	// 绕y轴旋转
	R_y.at<double>(0, 0) = cos(beta);
	R_y.at<double>(0, 2) = sin(beta);
	R_y.at<double>(2, 0) = -sin(beta);
	R_y.at<double>(2, 2) = cos(beta);
 
	// 第二次绕z轴旋转
	R_z.at<double>(0, 0) = cos(gamma);
	R_z.at<double>(0, 1) = -sin(gamma);
	R_z.at<double>(1, 0) = sin(gamma);
	R_z.at<double>(1, 1) = cos(gamma);
 
	return R_z * R_y * R_x;
}

void ListDir(std::string dirpath, std::vector<std::string>& files)
{
	// std::string path = "/home/dell3660/vscodeprojects/calibration/hand_eye_calibrate-main/collect_data"; // 替换为你的目录路径

    try {
        for (const auto& entry : std::filesystem::directory_iterator(dirpath)) {
            if (entry.is_regular_file()) { // 只处理普通文件，不包括目录等
                std::cout << entry.path().filename().string() << std::endl;
            }
        }
    } catch (const std::filesystem::filesystem_error& e) {
        std::cerr << "Filesystem error: " << e.what() << std::endl;
    } catch (const std::exception& e) {
        std::cerr << "General error: " << e.what() << std::endl;
    }

    // return 0;
}

void CameraCalibration(std::string imagePath, cv::Size& boardSize, cv::Size& imageSize)
{

	// 生成真实世界物理坐标
    std::vector<cv::Point3f> RealCoordinates;
    for (int y=0; y<boardSize.height; y++)
	{
        for (int x=0; x<boardSize.width; x++)
		{
            RealCoordinates.push_back(cv::Point3f(x*10, y*10, 0.0f));  // 间隔10mm
        }
    }
	std::vector<std::string> imagePathList;
	for (const auto& entry : std::filesystem::directory_iterator(imagePath))
	{
		if (entry.is_regular_file())
		{
			imagePathList.push_back(entry.path().string());
		}
	}

	int image_num = imagePathList.size();

	std::vector<std::vector<cv::Point3f>> objectPoints;
	std::vector<cv::Point2f> corners;  // 缓存每幅图检测到的角点
	std::vector<std::vector<cv::Point2f>> imagePoints;  // 保存检测到的所有角点
	for (int i = 0; i < image_num; i++)
	{
		cv::Mat img = cv::imread(imagePathList.at(i), cv::IMREAD_GRAYSCALE);
		bool foundCorners = cv::findChessboardCorners(img, boardSize, corners, cv::CALIB_CB_ADAPTIVE_THRESH+cv::CALIB_CB_NORMALIZE_IMAGE);
		if (foundCorners)
		{
			cv::cornerSubPix(img, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001));

            if (corners.size() == boardSize.area())
			{
                objectPoints.push_back(RealCoordinates);
                imagePoints.push_back(corners);
            }
		}
	
	}

	cv::Mat cameraMatrix = cv::Mat::zeros(3, 3, CV_64F);
    cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F);
    cv::Mat R = cv::Mat::zeros(3, 3, CV_64F);
    cv::Mat T = cv::Mat::zeros(3, 1, CV_64F);
    cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, R, T);

	std::cout << R.rows << " " << R.cols << std::endl;
	std::cout << T.rows << " " << T.cols << std::endl;
	// cv::FileStorage fs("camera_params.yaml", cv::FileStorage::WRITE);
	// fs << "camera_matrix" << cameraMatrix;
	// fs << "distortion_coefficients" << distCoeffs;
	// fs << "rotation_matrix" << R;
	// fs << "translation_vector" << T;
	// fs.release();
	
}
